import SimMath
import CPAModel
import math

    # 隶属度计算,隶属函数分为三类
    # a,b,c为三角形隶属度函数的三个顶点，x为参数（相对距离、相对速度、DCPA、TCPA）的实际值，y为一个评语（隶属于低、中、高碰撞风险）的隶属度
def membership1(x, b, c):
    if x < b:
        y = 1
    elif x <= c:
        y = (c - x) / (c - b)
    else:
        y = 0
    return y

def membership2(x, a, b, c):
    if x < a:
        y = 0
    elif x <= b:
        y = (x - a) / (b - a)
    elif x <= c:
        y = (c - x) / (c - b)
    else:
        y = 0
    return y

def membership3(x, a, b):
    if x < a:
        y = 0
    elif x <= b:
        y = (x - a) / (b - a)
    else:
        y = 1
    return y

def calcRelativeSpeed(head_ferry,speed_ferry,head_TS,speed_TS):
    # 计算航向差
    Cr = abs(head_TS - head_ferry)
    # 计算相对航速
    Vr = (speed_ferry ** 2 + speed_TS ** 2 - 2 * speed_ferry * speed_TS *
          math.cos(Cr / 180 * math.pi)) ** 0.5
    return Vr

# 此处更改暂时未使用，TestShip.py中ferry船舶碰撞风险计算仍然使用TestShip.py文件中 对象内的方法
def CollisionRisk(lon_ferry, lat_ferry, head_ferry, speed_ferry,
                  lon_TS, lat_TS, head_TS, speed_TS): # 蔡明佑负责开发
    # 计算此时渡船与一艘目标船的碰撞风险
    # 当前时刻渡船的数据
    #    lon_ferry = shipObject.lon
    #    lat_ferry = shipObject.lat
    #    head_ferry = shipObject.head
    #    speed_ferry = shipObject.speed
    # 计算两船之间的相对距离
    Dr = SimMath.calcDistanceHaversine(lon_ferry, lat_ferry, lon_TS, lat_TS)
    # 计算相对航速
    Vr = calcRelativeSpeed(head_ferry, speed_ferry, head_TS, speed_TS)
    DCPA, TCPA = CPAModel.calcCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
    # 根据熵权法处理历史会遇场景得到相对距离、相对速度、DCPA、TCPA的指标权重
    weight = [0.23615185, 0.0098181, 0.34989752, 0.40413254]

    # 修改一
    if TCPA < 0:
        weight = [0.61316688, 0.0098181, 0.17494876, 0.20206627]
    if Dr > 500:
        return 0
    # 计算DCPA
    # DCPA = shipObject.calcDCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
    # 计算TCPA
    # TCPA = shipObject.calcTCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
    # 根据各个指标取值计算其隶属于低、中、高碰撞风险的隶属度，列表中各值依次代表该指标隶属于低、中、高风险的隶属度
    
    # 修改二
    # 求相对距离的隶属度
    DCPA,TCPA = map(abs, [DCPA, TCPA])
    membership_Dr = [membership3(Dr, 200, 500), membership2(Dr, 50, 200, 500),
                        membership1(Dr, 50, 200)]
    # 求相对速度的隶属度
    membership_Vr = [membership1(Vr, 2, 6), membership2(Vr, 2, 6, 8), membership3(Vr, 6, 8)]
    # 求DCPA的隶属度
    membership_DCPA = [membership3(DCPA, 100, 300), membership2(DCPA, 50, 100, 300),
                        membership1(DCPA, 50, 100)]
    # 求TCPA的隶属度
    membership_TCPA = [membership3(TCPA, 60, 120), membership2(TCPA, 15, 60, 120),
                        membership1(TCPA, 15, 60)]
    # 根据各个指标的权重和隶属度合成此时碰撞风险隶属于低、中、高风险的隶属度
    # 低风险隶属度
    membership_low_risk = membership_Dr[0] * weight[0] + membership_Vr[0] * weight[1] + membership_DCPA[0] * \
                            weight[2] + membership_TCPA[0] * weight[3]
    # 中风险隶属度
    membership_medium_risk = membership_Dr[1] * weight[0] + membership_Vr[1] * weight[1] + membership_DCPA[1] * \
                                weight[2] + membership_TCPA[1] * weight[3]
    # 高风险隶属度
    membership_high_risk = membership_Dr[2] * weight[0] + membership_Vr[2] * weight[1] + membership_DCPA[2] * \
                            weight[2] + membership_TCPA[2] * weight[3]
    membership_risk = [membership_low_risk, membership_medium_risk, membership_high_risk]
    # 归一化
    membership_risk = [i / sum(membership_risk) for i in membership_risk]
    # 设低、中、高风险的的权重分别为0,0.5,1，进行去模糊化
    risk = membership_risk[0] * 0 + membership_risk[1] * 0.5 + membership_risk[2] * 1
    return risk

r = CollisionRisk(123.0998, 32.4, 21, 9, 123.1000221, 32.4, 43, 7)
print(r)